package org.opentcs.kcvehicle.communication.kc.udp.agv.param.function.navigation;

import io.netty.buffer.ByteBuf;
import io.netty.buffer.Unpooled;
import org.opentcs.kcvehicle.communication.kc.common.byteutils.ByteUtils;

public class Path {
  //序列号,4个字节,Integer,用于定位段在整个任务中的位置，从 1 开始奇数递增（如 1、3、5......）（point 2 4 6，这样path 和point就可以结合起来了），以区分同一个段 ID 在任务中是否多次出现
  private byte[] serialNum;
  //段 ID,4个字节,Integer
  private byte[] pathId;
  //指定角度时路径点的车头角度,4个字节,float ,当 isSpecifyAngle==1 的时候这个字段才生效
  private byte[] angle;
  //是否指定路径点角度,1个字节,byte,1 表示指定，0 表示不指定
  private byte isSpecifyAngle;

  //行驶姿态,1个字节,byte
  private byte drivePose;
  //任务中边上动作信息个数,1个字节,byte
  private byte pathActionSize;
  //保留,1个字节
  private byte pathRetain;
  //指定的目标最大速度,4个字节,float
  private byte[] maxSpeed;
  //指定的目标最大角速度,4个字节,float
  private byte[] maxAngularSpeed;
  //预留，4个字节
  private byte[] pathRetain2;

  //任务中点上动作结构体,这里数组的长度是 pathActionSize 长度
  private Action[] actions;


  public Path(
      Integer serialNum, Integer pathId, float angle, byte isSpecifyAngle, byte drivePose,
      byte pathActionSize, float maxSpeed, float maxAngularSpeed, Action[] actions
  ) {
    this.serialNum = ByteUtils.intToBytes(serialNum, 1);
    this.pathId = ByteUtils.intToBytes(pathId, 1);
    this.angle = ByteUtils.floatToBytes(angle);
    this.isSpecifyAngle = isSpecifyAngle;
    this.drivePose = drivePose;
    this.pathActionSize = pathActionSize;
    this.pathRetain = (byte) 0x00;
    this.maxSpeed = ByteUtils.floatToBytes(maxSpeed);
    this.maxAngularSpeed = ByteUtils.floatToBytes(maxAngularSpeed);
    this.pathRetain2 = new byte[]{(byte) 0x00, (byte) 0x00, (byte) 0x00, (byte) 0x00};
    this.actions = actions;
  }

  public byte[] toBytes() {
    ByteBuf byteBuf = Unpooled.buffer();
    byteBuf.writeBytes(serialNum);
    byteBuf.writeBytes(pathId);
    byteBuf.writeBytes(angle);
    byteBuf.writeByte(isSpecifyAngle);
    byteBuf.writeByte(drivePose);
    byteBuf.writeByte(pathActionSize);
    byteBuf.writeByte(pathRetain);
    byteBuf.writeBytes(maxSpeed);
    byteBuf.writeBytes(maxAngularSpeed);
    byteBuf.writeBytes(pathRetain2);
    if (actions != null) {
      for (Action action : actions) {
        byteBuf.writeBytes(action.toBytes());
      }
    }
    int i = byteBuf.writerIndex();
    byte[] bytes = new byte[i];
    byteBuf.readBytes(bytes);
    return bytes;
  }

}
